function [pL2S1,pL2S2,pL2S3]=servoL2(AngShoulder,AngLong,AngFoot)
    S1=pi/2+AngShoulder;
    S2=-AngLong;
    S3=AngFoot+pi;
    %计算单腿三个舵机角度
    pL2S1=round(S1*2000/pi+500);
    if (pL2S1<500)
        pL2S1=500;
    elseif (pL2S1>2500)
        pL2S1=2500;
    end
    %
    pL2S2=round(S2*2000/pi+500);
    if (pL2S2<500)
        pL2S2=500;
    elseif (pL2S2>2500)
            pL2S2=2500;
    end
    %
    pL2S3=round(S3*2000/pi+500);
    if (pL2S3<500)
        pL2S3=500;
    elseif (pL2S3>2500)
            pL2S3=2500;
    end
end